#pragma once
#include <iostream>
#include <Eigen/Dense>
#include <memory>
#include "continuous_linear_system.h"
#include "discrete_linear_system.h"
#include "kf.h"
using namespace KalmanFilter;
namespace auto_ros
{
namespace control
{
class KfLateral
{
public:
	//init state fun when speed is 1
	KfLateral(double mass, double cf, double cr, double lf, double lr, double iz, double rate,
			  Eigen::MatrixXd &sys_covariance, Eigen::MatrixXd &measure_covariance);
	bool speed_update_sys(double speed);
	bool step(Eigen::VectorXd &control_last, Eigen::VectorXd &y, double speed);
	bool reset(Eigen::VectorXd state_esti, Eigen::MatrixXd state_cov_esti);
	double mass();
	double lf();
	double lr();
	double cf();
	double cr();
	double iz();
	//const std::shared_ptr<ContinuousLinearSys> con_ptr();
	double speed_threshold();
	bool set_mass(double mass);
	bool set_lf(double lf);
	bool set_lr(double lr);
	bool set_cf(double cf);
	bool set_cr(double cr);
	bool set_iz(double iz);
	Eigen::VectorXd state_esti();
	Eigen::MatrixXd state_cov_esti();
	std::shared_ptr<ContinuousLinearSys> con_ptr_;
	std::shared_ptr<DiscreteLinearSys> dis_ptr_;
	std::shared_ptr<KF> kf_ptr_;

private:
	//paramter
	double mass_;
	double cf_;
	double cr_;
	double lf_;
	double lr_;
	double iz_;
	double h_;
	//local parameter
	double c_yaw_rate_;
	double c_beta_;
	double c_delta_;
	double d_yaw_rate_;
	double d_delta_;
	double d_beta_;

	double rate_;

	//state
	double speed_;
	double speed_threshold_; // divided by 0 error
};
} // namespace control
} // namespace auto_ros